#include <stdio.h>
#include <assert.h>
#include <math.h>
#include "quaternion.h"

void test_quaternion() {
    printf("=== 测试 Quaternion ===\n");
    
    // 测试单位四元数
    Quaternion q1 = quaternion_identity();
    assert(q1.w == 1.0 && q1.x == 0.0 && q1.y == 0.0 && q1.z == 0.0);
    
    // 测试轴角转换
    Vector3D axis = vector3d_create(0.0, 0.0, 1.0);
    Quaternion q2 = quaternion_from_axis_angle(axis, PI/2);
    assert(fabs(q2.w - cos(PI/4)) < 1e-6);
    assert(fabs(q2.z - sin(PI/4)) < 1e-6);
    
    // 测试旋转
    Vector3D v = vector3d_create(1.0, 0.0, 0.0);
    Vector3D rotated = quaternion_rotate_vector(q2, v);
    assert(fabs(rotated.x) < 1e-6);
    assert(fabs(rotated.y - 1.0) < 1e-6);
    
    // 测试欧拉角转换
    Quaternion q3 = quaternion_from_euler(0.0, 0.0, PI/2);
    Vector3D euler = quaternion_to_euler(q3);
    assert(fabs(euler.z - PI/2) < 1e-6);
    
    // 测试SLERP
    Quaternion q4 = quaternion_slerp(q1, q3, 0.5);
    Vector3D mid_euler = quaternion_to_euler(q4);
    assert(fabs(mid_euler.z - PI/4) < 1e-6);
    
    printf("所有Quaternion测试通过！\n");
}

int main() {
    test_quaternion();
    return 0;
}
